#include "physical.h"
#include "odebodywrap.h"
//#include <dtCore/collisioncategorydefaults.h>
#include "transform.h"
#include <ode/collision.h>
#include <ode/objects.h>

namespace CoreBase
{
	Physical::Physical( ActorBase* owneractor,const QString& name ):Transformable(owneractor,name),_bodyWrap(new ODEBodyWrap())
	{
		ctor();
	}

	Physical::Physical(osg::MatrixTransform& node,ActorBase* owneractor, const QString& name ):Transformable(node
		,owneractor, name),_bodyWrap(new ODEBodyWrap())
	{
		ctor();
	}

	Physical::~Physical()
	{
		_bodyWrap = NULL;
	}

	void Physical::setBodyID( dBodyID bodyID )
	{
		_bodyWrap->setBodyID(bodyID);
		if( bodyID != 0 )
		{
			// Copy position and rotation of geometry over to
			const dReal* position = dGeomGetPosition( getGeomID() );
			_bodyWrap->setPosition(osg::Vec3(position[0], position[1], position[2]));
			const dReal* rotation = dGeomGetRotation( getGeomID() );
			osg::Matrix mat;
			mat(0,0) = rotation[0];
			mat(1,0) = rotation[1];
			mat(2,0) = rotation[2];
			mat(0,1) = rotation[4];
			mat(1,1) = rotation[5];
			mat(2,1) = rotation[6];
			mat(0,2) = rotation[8];
			mat(1,2) = rotation[9];
			mat(2,2) = rotation[10];
			_bodyWrap->setRotation(mat);
		}
		dGeomSetBody(getGeomID(),bodyID);
	}

	dBodyID Physical::getBodyID() const
	{
		return _bodyWrap->getBodyID();
	}

	void Physical::enableDynamics(bool enable)
	{
		_bodyWrap->enableDynamics(enable);
	}

	bool Physical::dynamicsEnabled() const
	{
		return _bodyWrap->dynamicsEnabled();
	}

	void Physical::setMass(const dMass* mass)
	{
		_bodyWrap->setMass(mass);
	}

	void Physical::getMass(dMass* mass) const
	{
		_bodyWrap->getMass(mass);
	}

	void Physical::setMass(float mass)
	{
		_bodyWrap->setMass(mass);
	}

	float Physical::getMass() const
	{
		return _bodyWrap->getMass();
	}

	void Physical::setCenterOfGravity(const osg::Vec3& centerOfGravity)
	{
		_bodyWrap->setCenterOfGravity(centerOfGravity);
	}

	void Physical::getCenterOfGravity(osg::Vec3& dest) const
	{
		_bodyWrap->getCenterOfGravity(dest);
	}

	void Physical::setInertiaTensor(const osg::Matrix& inertiaTensor)
	{
		_bodyWrap->setInertiaTensor(inertiaTensor);
	}

	void Physical::getInertiaTensor(osg::Matrix& dest) const
	{
		_bodyWrap->getInertiaTensor(dest);
	}

	void Physical::prePhysicsStepUpdate()
	{
		if (_bodyWrap->dynamicsEnabled())
		{
			Transform transform;
			this->getTransform(transform, Transformable::ABS_CS);
			_bodyWrap->updateBodyTransform(transform);
		}
		else
		{
			Transformable::prePhysicsStepUpdate();
		}
	}

	void Physical::postPhysicsStepUpdate()
	{
		if( dynamicsEnabled() )
		{
			const dReal* position = dBodyGetPosition(_bodyWrap->getBodyID());
			const dReal* rotation = dBodyGetRotation(_bodyWrap->getBodyID());
			osg::Matrix newRotation;
			newRotation(0,0) = rotation[0];
			newRotation(1,0) = rotation[1];
			newRotation(2,0) = rotation[2];
			newRotation(0,1) = rotation[4];
			newRotation(1,1) = rotation[5];
			newRotation(2,1) = rotation[6];
			newRotation(0,2) = rotation[8];
			newRotation(1,2) = rotation[9];
			newRotation(2,2) = rotation[10];
			Transform newTransform;
			newTransform.setTranslation(position[0], position[1], position[2]);
			newTransform.setRotation(newRotation);
			this->setTransform(newTransform);
		}
	}

	void Physical::ctor()
	{
		//SetCollisionCategoryBits(COLLISION_CATEGORY_MASK_PHYSICAL);
	}
}